#ifndef APL_PID_H
#define APL_PID_H

#include "stm32f4xx.h"

typedef struct {
	float KP, KI, KD;
} PID_K_t;

extern PID_K_t k_speed_chassis, k_speed_gimbal, k_angle_gimbal;

#define KP_speed 15
#define KI_speed 1.15
#define KD_speed 8

#define KP_angle 0.1
#define KI_angle 0
#define KD_angle 0

typedef struct _PID {
    float kp, ki, kd;
    float err, lastErr;
    float integral, maxIntegral;
    float output, maxOutput;
} PID;

extern void  PID_init_speed(PID *pid, float MAX_OUTPUT, PID_K_t *K);
extern void PID_init_angle(PID *pid, float MAX_OUTPUT, PID_K_t *K);

float PID_speed_output(PID* pid, float target, float feedback);
float PID_angle_output(PID *pid, float target, float feedback);

#endif
